#ifndef URCONTROLCAMERA_HPP
#define URCONTROLCAMERA_HPP
#include "urcontroloffseta.hpp"
#include "usbcamera.hpp"
#include <algorithm>

#define CALIBCAM
#ifdef CALIBCAM
#include "findmarker.hpp"
#endif

class URControlCamera : public URControlOffsetA
{
public:


    static cv::Vec3d PointAtPlane(cv::Vec2d pixel, const cv::Mat &K, const cv::Mat &D, cv::Vec4d plane)
    {
        std::vector<cv::Vec2d> pixels(1,pixel);
        std::vector<cv::Vec2d> rays;
        cv::undistortPoints(pixels, rays, K, D);
        cv::Vec3d ray(rays[0][0], rays[0][1], 1);

        double k=-plane[3]/(ray.ddot(cv::Vec3d(plane[0],plane[1],plane[2])));
        return ray*k;
    }


    static cv::Vec6d PointAtPlanePose(cv::Vec2d pixelOrigin, cv::Vec2d pixelXPlus, cv::Vec6d finger2Head,
                                      const cv::Mat &K, const cv::Mat &D, cv::Vec4d plane, cv::Vec6d camera2Base)
    {
        cv::Vec3d ptOrigin=PointAtPlane(pixelOrigin, K, D, plane);
        cv::Vec3d ptXPlus=PointAtPlane(pixelXPlus, K, D, plane);

        cv::Vec3d ptXUnit=ptXPlus-ptOrigin;
        double xLength=cv::norm(ptXUnit);
        ptXUnit/=xLength;

        cv::Vec3d ptZUnit(plane[0],plane[1],plane[2]);

        cv::Vec3d ptYUnit=ptZUnit.cross(ptXUnit);

        cv::Vec3d ptCam[4]={
            ptOrigin, ptOrigin+ptXUnit, ptOrigin+ptYUnit, ptOrigin+ptZUnit
        };

        cv::Vec3d ptBase[4];

        for(int i=0;i<4;i++)
            ptBase[i]=HeadToBase(ptCam[i],camera2Base);

        cv::Vec3d ptHead[4]={
            cv::Vec3d(0,0,0),
            cv::Vec3d(1,0,0),
            cv::Vec3d(0,1,0),
            cv::Vec3d(0,0,1)
        };

        for(int i=0;i<4;i++)
            ptHead[i]=HeadToBase(ptHead[i], finger2Head);


        return ComputeTransform(ptHead,ptBase);

    }





    static void RectangleCornerToCenterXPlus(cv::Vec2d corner[4], cv::Vec2d &center, cv::Vec2d &xPlus)
    {
        center=RectangleCornerToCenter(corner);

        cv::Vec2d xUnit=corner[1]-corner[0];
        //       cv::Vec3d xAxis=LineEquation(corner[0],corner[1]);

        //        cv::Vec2d xAxisD(xAxis[0],xAxis[1]);

        //        double distance2=fabs(xAxisD.ddot(corner[2])+xAxis[2])/(cv::norm(xAxisD));
        //        double distance3=fabs(xAxisD.ddot(corner[3])+xAxis[2])/(cv::norm(xAxisD));

        //        double openWidth=distance2>distance3?distance2:distance3;

        //        //length of xUnit is open width
        //        xUnit*=openWidth/cv::norm(xUnit);
        xPlus=center+xUnit;
    }


    static void PickRectangle0(double openmm, cv::Vec2d corner[4], cv::Vec6d finger2Head,
    const cv::Mat &K, const cv::Mat &D, cv::Vec4d plane, cv::Vec6d camera2Base,
    const char *ipUR, const char *ipPC, double a, double v)
    {
        cv::Vec2d center;
        cv::Vec2d xplus;

        RectangleCornerToCenterXPlus(corner, center, xplus);

        double h1=0.05;

        double h2=0.01;

        finger2Head[2]+=h1;

        cv::Vec6d urp=PointAtPlanePose(center,xplus,finger2Head,K,D,plane,camera2Base);

        finger2Head[2]-=h1;

        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5], a, v);

        URControl::RG2(ipUR, ipPC, openmm);

        finger2Head[2]+=h2;

        urp=PointAtPlanePose(center,xplus,finger2Head,K,D,plane,camera2Base);

        finger2Head[2]-=h2;

        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5], a, v);

        URControl::RG2(ipUR, ipPC, 0);

        finger2Head[2]+=h1;

        urp=PointAtPlanePose(center,xplus,finger2Head,K,D,plane,camera2Base);

        finger2Head[2]-=h1;

        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5], a, v);
#if 1
        cv::Vec6d droppos(0.144314,-0.38248,0.303411,0.806607,-1.45468,-0.400633);

        URControl::SetTCPPose(ipUR, ipPC, droppos[0],droppos[1],droppos[2],droppos[3],droppos[4],droppos[5], a, 0.5);

        URControl::RG2(ipUR, ipPC, openmm);

        cv::Vec6d idlepos(0.0841253,-0.300965,0.425928,-1.28698,2.80417,-0.112645);

        URControl::SetTCPPose(ipUR, ipPC, idlepos[0],idlepos[1],idlepos[2],idlepos[3],idlepos[4],idlepos[5], a, 0.5);

        URControl::RG2(ipUR, ipPC, 0);
#endif

    }

    static void RectangleCornerToCubois(cv::Vec3d &centroid, cv::Vec3d &size, cv::Vec3d baseAxis[3], cv::Vec2d corner[4], const cv::Mat &K, const cv::Mat &D, cv::Vec4d plane)
    {
        cv::Vec3d ptCorner[4];
        for(int i=0;i<4;i++)
            ptCorner[i]=PointAtPlane(corner[i], K, D, plane);

        cv::Vec3d originAtPlane=ptCorner[0];

        cv::Vec3d planeNormal(plane[0],plane[1],plane[2]);

        baseAxis[2]=planeNormal/cv::norm(planeNormal);

        cv::Vec3d xAxisAtPlane=ptCorner[1]-ptCorner[0];

        baseAxis[0]=xAxisAtPlane/cv::norm(xAxisAtPlane);

        baseAxis[1]=baseAxis[2].cross(baseAxis[0]);


        cv::Vec3d ptCornerAtPlane[4];
        for(int i=0;i<4;i++)
            ptCornerAtPlane[i]=ptCorner[i]-originAtPlane;


        cv::Matx23d projection(
                    baseAxis[0][0],baseAxis[0][1],baseAxis[0][2],
                baseAxis[1][0],baseAxis[1][1],baseAxis[1][2]
                );

        cv::Vec2d cornerAtPlane[4];
        for(int i=0;i<4;i++)
            cornerAtPlane[i]=projection*ptCornerAtPlane[i];

        cv::Vec2d minAtPlane=cornerAtPlane[0];
        cv::Vec2d maxAtPlane=cornerAtPlane[0];

        for(int i=1;i<4;i++)
        {
            for(int j=0;j<2;j++)
            {
                if(cornerAtPlane[i][j]<minAtPlane[j])
                    minAtPlane[j]=cornerAtPlane[i][j];
                else
                    if(cornerAtPlane[i][j]>maxAtPlane[j])
                        maxAtPlane[j]=cornerAtPlane[i][j];
            }
        }


        cv::Vec2d centerAtPlane=(minAtPlane+maxAtPlane)/2;

        cv::Vec2d sizeAtPlane=(maxAtPlane-minAtPlane)/2;

        centroid=originAtPlane+centerAtPlane[0]*baseAxis[0]+centerAtPlane[1]*baseAxis[1];

        size=cv::Vec3d(sizeAtPlane[0],sizeAtPlane[1],0.018/2);

    }






    static void PickRectangle(double &openmm, cv::Vec2d corner[4], cv::Vec6d finger2Head,
    const cv::Mat &K, const cv::Mat &D, cv::Vec4d plane, cv::Vec6d camera2Base,
    const char *ipUR, const char *ipPC, double a, double v)
    {

        cv::Vec3d cuboisCentroid;
        cv::Vec3d cuboisSize;
        cv::Vec3d baseAxis[3];

        RectangleCornerToCubois(cuboisCentroid, cuboisSize, baseAxis, corner, K, D, plane);

        double targetHalfWidthmm=cuboisSize[1]*1000;

        double openZOffsetmm=URScript::RGZOffset(targetHalfWidthmm);

        openmm=(targetHalfWidthmm*2)+10;

         std::cout<<"openmm="<<openmm<<"\n"<<std::flush;

        double h=0.05;

        finger2Head[2]+=cuboisSize[2]*2+h;

        cv::Vec6d urp=URControlOffset::PointAtPlanePose(cuboisCentroid,baseAxis,finger2Head,camera2Base);

        finger2Head[2]-=cuboisSize[2]*2+h;

        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5], a, v);

        std::cout<<"m1\n"<<std::flush;

        URControl::RG2(ipUR, ipPC, openmm);

        std::cout<<"m2\n"<<std::flush;

        finger2Head[2]+=cuboisSize[2]-(openZOffsetmm/1000);

        urp=URControlOffset::PointAtPlanePose(cuboisCentroid,baseAxis,finger2Head,camera2Base);

        finger2Head[2]-=cuboisSize[2]-(openZOffsetmm/1000);

        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5], a, v);

        std::cout<<"m3\n"<<std::flush;

        URControl::RG2(ipUR, ipPC, 0);

        std::cout<<"m4\n"<<std::flush;

        finger2Head[2]+=cuboisSize[2]*2+h;

        urp=URControlOffset::PointAtPlanePose(cuboisCentroid,baseAxis,finger2Head,camera2Base);

        finger2Head[2]-=cuboisSize[2]*2+h;

        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5], a, v);

        std::cout<<"m5\n"<<std::flush;


#if 0
//        cv::Vec6d droppos(0.149714,-0.372176,0.189966,0.851806,-1.40524,-0.419454);

        cv::Vec6d droppos(-0.233127,-0.19301,0.389471,1.29641,-2.84168,0.0221463);

        URControl::SetTCPPose(ipUR, ipPC, droppos[0],droppos[1],droppos[2],droppos[3],droppos[4],droppos[5], a, 0.5);

        URControl::RG2(ipUR, ipPC, openmm);

//        cv::Vec6d idlepos(0.0841253,-0.300965,0.425928,-1.28698,2.80417,-0.112645);

//        URControl::SetTCPPose(ipUR, ipPC, idlepos[0],idlepos[1],idlepos[2],idlepos[3],idlepos[4],idlepos[5], a, 0.5);

        URControl::RG2(ipUR, ipPC, 0);
#endif

    }






    static bool LoadVec6d(std::string fp, cv::Vec6d &v)
    {
        cv::Mat m;
        if(Load(m,fp))
        {
            for(int i=0;i<6;i++)
                v[i]=m.at<double>(i);
            return true;
        }
        return false;
    }

    static bool LoadVec4d(std::string fp, cv::Vec4d &v)
    {
        cv::Mat m;
        if(Load(m,fp))
        {
            for(int i=0;i<4;i++)
                v[i]=m.at<double>(i);
            return true;
        }
        return false;
    }



    USBCamera c;

    cv::Vec6d camera2Base;

    cv::Vec4d plane;

public:
    URControlCamera()
    {
        c.D=(cv::Mat_<double>(5,1)<<-5.2350148524189377e-001, 1.1727596392200465e-001,
             9.9529411655092177e-004, 1.9872735353972520e-003, 0.);

        c.K=(cv::Mat_<double>(3,3)<<9.2242145575986262e+002, 0., 3.0620176294143153e+002, 0.,
             9.2242145575986262e+002, 2.2249184398617811e+002, 0., 0., 1.);

        //        camera2Base=cv::Vec6d(0.245858, 0.0370281, 0.688829, -0.0179064, 2.94312, -0.130577);

        //camera2Base=cv::Vec6d(0.318645, 0.0614684, 0.687712, 0.0771025, 3.11192, -0.076169);

        //camera2Base=cv::Vec6d(0.323387, 0.0590507, 0.688601, 0.0803636, 3.1246, -0.0671326);

//        camera2Base=cv::Vec6d(0.314146, 0.0431733, 0.691222, 0.0742956, 3.10598, -0.0756126);

//        camera2Base=cv::Vec6d(0.321542, 0.0399183, 0.693801, 0.0724028, 3.11987, -0.067536);

        camera2Base=cv::Vec6d(-0.374073, 0.00620067, 0.674188, 0.0175234, 3.12623, -0.0502706);

        camera2Base=cv::Vec6d(-0.399302, 0.00995373, 0.673098, 0.0247431, 3.13135, 0.0037845);

        camera2Base=cv::Vec6d(-0.399945, 0.00470522, 0.673758, 0.0110598, 3.12517, 0.00730078);

        camera2Base=cv::Vec6d(-0.396324, 0.00430994, 0.67368, 0.0103077, 3.13233, 0.00878492);


        camera2Base=cv::Vec6d(-0.449917, 0.0431695, 0.674897, 0.00413957, 3.13786, 0.0187029);

        //        plane=cv::Vec4d(-0.0357797, 0.0232109, 0.99909, -0.681377);

//        plane=cv::Vec4d(0.0289973, 0.0491297, 0.998371, -0.679425);

//        plane=cv::Vec4d(-0.00530247, 0.0567132, 0.998376, -0.681697);

        plane=cv::Vec4d(0.0285008, 0.0140821, 0.999495, -0.662782);
        plane=cv::Vec4d(-0.00614975, 0.0245527, 0.99968, -0.66136);

        plane=cv::Vec4d(-0.00563396, -0.0244308, 0.999686, -0.663363);
        plane=cv::Vec4d(-0.036623, -0.0334931, 0.998768, -0.659779);

        plane=cv::Vec4d(-0.0190046, -0.0107309, 0.999762, -0.665201);

        plane=cv::Vec4d(-0.0130265, 0.0374456, 0.999214, -0.659777);
    }


    void PointToPlane(cv::Vec2d pixelOrigin, cv::Vec2d pixelXPlus, cv::Vec6d finger2Head,
    const char *ipUR, const char *ipPC, double a, double v)
    {
        cv::Vec6d urp=PointAtPlanePose(pixelOrigin, pixelXPlus, finger2Head,
                c.K,c.D,plane,camera2Base);
        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5], a, v);
    }


    void PointToPlane(cv::Vec2d pixelOrigin, cv::Vec6d finger2Head, const char *ipUR, const char *ipPC, double a, double v)
    {
        PointToPlane(pixelOrigin, pixelOrigin+cv::Vec2d(1,0), finger2Head, ipUR, ipPC, a, v);
    }


#ifdef CALIBCAM

    bool CalibratePosition(float markerLength=36.0/8*6/1e3, cv::Vec6d marker2head=cv::Vec6d(0,0,5.5/1e3,0,0,CV_PI/4))
    {
        c.open(0);
        c.set(cv::CAP_PROP_SETTINGS,1);

        FindMarker fm(0);

        cv::Mat m;

        cv::Mat m32;

        cv::Mat sm;

        int n=0;

        cv::namedWindow("m",cv::WINDOW_NORMAL);
        int k=0;
        while(k!=27)
        {
            if(c.read(m))
            {
                n++;
#if 0
                m.convertTo(m32,CV_32S);

                if(sm.empty())
                    m32.copyTo(sm);
                else
                    sm+=m32;

                sm.convertTo(m,CV_8U,1.0/n);
#endif

                //                cv::blur(m,m,cv::Size(3,3));

                // detect markers and estimate pose
                cv::aruco::detectMarkers(m, fm.dictionary, fm.corners, fm.ids, fm.detectorParams, fm.rejected);

                if(!fm.ids.empty())
                {
                    cv::aruco::estimatePoseSingleMarkers(fm.corners, markerLength, c.K, c.D, fm.rvecs, fm.tvecs);

                    cv::Vec6d urp;

                    char *ipUR="192.168.1.13";
                    char *ipPC="192.168.1.14";

                    URControl::GetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5]);

                    cv::Vec6d mt(fm.tvecs[0][0], fm.tvecs[0][1], fm.tvecs[0][2], fm.rvecs[0][0], fm.rvecs[0][1], fm.rvecs[0][2]);



                    fm.Print();



                    cv::Vec6d ftf=ComposeTransform(ComposeTransform(ReverseTransform(mt),marker2head),urp);

                    std::cout<<"mt="<<mt<<"\n";

                    std::cout<<"ftf="<<ftf<<"\n"<<std::flush;

                }
                cv::Mat canvas=m.clone();
                c.CenterCross(canvas);

                fm.DrawResult(canvas,c.K,c.D,markerLength);

                cv::imshow("m",canvas);
                k=cv::waitKey(1);
            }
        }


        c.release();

        return true;
    }


    bool CalibratePlane(float markerLength=0.120)
    {
        c.open(1);

        FindMarker fm(0);

        cv::Mat m;

        cv::Mat m32;

        cv::Mat sm;

        int n=0;

        cv::namedWindow("m",cv::WINDOW_NORMAL);
        int k=0;
        while(k!=27)
        {
            if(c.read(m))
            {
                n++;
#if 1
                m.convertTo(m32,CV_32S);

                if(sm.empty())
                    m32.copyTo(sm);
                else
                    sm+=m32;

                sm.convertTo(m,CV_8U,1.0/n);
#endif
                // detect markers and estimate pose
                cv::aruco::detectMarkers(m, fm.dictionary, fm.corners, fm.ids, fm.detectorParams, fm.rejected);

                if(!fm.ids.empty())
                {
                    cv::aruco::estimatePoseSingleMarkers(fm.corners, markerLength, c.K, c.D, fm.rvecs, fm.tvecs);

                    cv::Vec6d mt(fm.tvecs[0][0], fm.tvecs[0][1], fm.tvecs[0][2], fm.rvecs[0][0], fm.rvecs[0][1], fm.rvecs[0][2]);

                    plane=MarkerPosToPlane(mt);
                    //                    std::cout<<"mt="<<mt<<"\n";

                    //                    std::cout<<"plane="<<plane<<"\n"<<std::flush;

                }
                cv::Mat canvas=m.clone();
                c.CenterCross(canvas);

                fm.DrawResult(canvas,c.K,c.D,markerLength);

                cv::imshow("press ESC to end",canvas);
                k=cv::waitKey(1);
            }
        }

        c.release();

        std::cout<<"plane=cv::Vec4d"<<plane<<";\n"<<std::flush;

        return true;
    }



    void play()
    {

        c.open(1);

        FindMarker fm(0);

        cv::Mat m;

        cv::Mat m32;

        cv::Mat sm;

        int n=0;

        cv::namedWindow("m",cv::WINDOW_NORMAL);
        int k=0;
        while(k!=27)
        {
            if(c.read(m))
            {
                n++;
#if 1
                m.convertTo(m32,CV_32S);

                if(sm.empty())
                    m32.copyTo(sm);
                else
                    sm+=m32;

                sm.convertTo(m,CV_8U,1.0/n);
#endif
                // detect markers and estimate pose
                cv::aruco::detectMarkers(m, fm.dictionary, fm.corners, fm.ids, fm.detectorParams, fm.rejected);

                cv::Mat canvas=m.clone();
                c.CenterCross(canvas);

                fm.DrawResult(canvas,c.K,c.D,150);

                cv::imshow("m",canvas);
                k=cv::waitKey(1);
            }
        }



        cv::Vec6d urp=PointAtPlanePose(cv::Vec2d(fm.corners[0][0].x, fm.corners[0][0].y),
                cv::Vec2d(fm.corners[0][1].x, fm.corners[0][1].y),
                cv::Vec6d(0,0,0.22,0,0,CV_PI/4),
                c.K,c.D,plane,camera2Base);

        char *ipUR="192.168.1.13";
        char *ipPC="192.168.1.14";

        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5]);

        cv::waitKey();

        cv::destroyWindow("m");

#if 0
        urp=cv::Vec6d(0.111001,-0.283316,0.295293,-2.86776,-0.939171,-0.0791106);

        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5]);
#endif
    }


    static bool CodeCorners(std::string path, cv::Vec2d xy[4], cv::Vec3d xyz[4],
    int id=0, float markerLength=27/1e3, cv::Vec6d marker2head=cv::Vec6d(0,0,5.5/1e3,0,0,-CV_PI/4))
    {
        cv::Mat m=cv::imread(path);
        if(m.empty())
            return false;
        FindMarker fm(0);

        // detect markers and estimate pose
        cv::aruco::detectMarkers(m, fm.dictionary, fm.corners, fm.ids, fm.detectorParams, fm.rejected);

        if(fm.ids.empty())
            return false;

        std::vector<int>::iterator it = std::find (fm.ids.begin(), fm.ids.end(), id);
        if (it == fm.ids.end())
            return false;

        size_t index=it-fm.ids.begin();
        for(int i=0;i<4;i++)
            xy[i]=cv::Vec2d(fm.corners[index][i].x, fm.corners[index][i].y);

        //          fm.Print();

        std::string ymlpath=path.substr(0,path.size()-3)+"yml";

        cv::Mat v;
        if(!Load(v,ymlpath))
            return false;

        cv::Vec6d tf;
        for(int i=0;i<6;i++)
            tf[i]=v.at<double>(i);

        xyz[0]=cv::Vec3d(-markerLength/2, markerLength/2, 0);
        xyz[1]=cv::Vec3d(markerLength/2, markerLength/2, 0);
        xyz[2]=cv::Vec3d(markerLength/2, -markerLength/2, 0);
        xyz[3]=cv::Vec3d(-markerLength/2, -markerLength/2, 0);

        for(int i=0;i<4;i++)
        {
            //              xyz[i]=HeadToBase(xyz[i],marker2head);
            xyz[i]=HeadToBase(HeadToBase(xyz[i],marker2head),tf);
            //            std::cout<<xyz[i]<<"\n"<<std::flush;
        }

        cv::Mat D=(cv::Mat_<double>(5,1)<<-5.2350148524189377e-001, 1.1727596392200465e-001,
                   9.9529411655092177e-004, 1.9872735353972520e-003, 0.);

        cv::Mat K=(cv::Mat_<double>(3,3)<<9.2242145575986262e+002, 0., 3.0620176294143153e+002, 0.,
                   9.2242145575986262e+002, 2.2249184398617811e+002, 0., 0., 1.);

        fm.DrawResult(m,K,D,markerLength);
        cv::imshow("press y to accept",m);
        int k=cv::waitKey(0);

        if(k!='y')
            return false;
        return true;

    }


    void LoadCorners(std::string folder,
                     int id=0, float markerLength=27/1e3, cv::Vec6d marker2head=cv::Vec6d(0,0,5.5/1e3,0,0,-CV_PI/4))
    {
        std::vector<std::string> filename;
        ListFileName(folder,filename);

        std::vector<cv::Vec2d> pixel;
        std::vector<cv::Vec3d> point;

        for(size_t i=0;i<filename.size();i++)
        {
            cv::Vec2d xy[4];
            cv::Vec3d xyz[4];
            if(CodeCorners(folder+filename[i], xy, xyz, id, markerLength, marker2head))
            {
                for(int j=0;j<4;j++)
                {
                    pixel.push_back(xy[j]);
                    point.push_back(xyz[j]);
                }
            }
        }

        std::cout<<"pixel="<<pixel.size()<<"\n";

        cv::Mat r,t;
        cv::Mat in;
        cv::solvePnPRansac(point,pixel,c.K,c.D,r,t,false,10000,2.0,0.999,in);


        cv::Vec6d base2Camera(t.at<double>(0),t.at<double>(1),t.at<double>(2),r.at<double>(0),r.at<double>(1),r.at<double>(2));


        //        std::cout<<"base2Camera="<<base2Camera<<"\n"<<std::flush;

        camera2Base=ReverseTransform(base2Camera);

        std::cout<<"camera2Base=cv::Vec6d"<<camera2Base<<";\n"<<std::flush;

        std::cout<<"in="<<in.size()<<"\n"<<std::flush;



    }


#endif

    void PickRectangle(double &openmm, cv::Vec2d corner[4], cv::Vec6d finger2Head,
    const char *ipUR, const char *ipPC, double a, double v)
    {
        PickRectangle(openmm, corner,finger2Head,c.K,c.D,plane,camera2Base,ipUR,ipPC,a,v);
    }

    void SampleOne(std::string folder=TimeString(2)+"/",std::string winname="frame", std::string ext=".png")
    {
        SetFolder(folder);
        c.open(1);

        cv::namedWindow(winname,cv::WINDOW_NORMAL);

        int k=0;
        cv::Mat m;
        while(k!=27)
        {
            if(c.read(m))
            {
                cv::imshow(winname, m);
                k=cv::waitKey(1);

                if(k=='c')
                {
                    std::string name=TimeString();
                    cv::imwrite(folder+name+ext,m);

                    char *ipUR="192.168.1.13";
                    char *ipPC="192.168.1.14";

                    cv::Vec6d urp1;
                    URControl::GetTCPPose(ipUR,ipPC,urp1[0],urp1[1],urp1[2],urp1[3],urp1[4],urp1[5]);

                    Save(cv::Mat(urp1),folder+name+".yml");

                    std::cout<<"save "<<name<<"\n"<<std::flush;
                }

            }
        }

        cv::destroyWindow(winname);

        c.release();

        LoadCorners(folder);

    }

    void Sample(std::string folder=TimeString(2)+"/", std::string winname="frame", std::string ext=".png")
    {
        SetFolder(folder);

        c.open(0);

        char *ipUR="192.168.1.13";
        char *ipPC="192.168.1.14";


        cv::Vec6d urps[]={
   #if 1
            cv::Vec6d(4.9023699999999998e-001, 1.4449100000000001e-001,
            9.8855899999999997e-002, 2.2281000000000001e-001,
            1.1926500000000000e-001, -2.2595999999999998e+000),
            cv::Vec6d(5.0092800000000004e-001, 7.7473600000000004e-002,
            9.4159699999999999e-002, 2.6108199999999998e-001,
            8.8017399999999996e-002, -2.3925700000000001e+000),
            cv::Vec6d(5.0158599999999998e-001, -1.7587700000000001e-002,
            9.2456899999999995e-002, 1.9244100000000000e-001,
            -2.1664300000000000e-001, -2.3960800000000000e+000),
            cv::Vec6d(4.4446900000000000e-001, -1.0794100000000000e-001,
            -4.1451800000000000e-003, 9.9575499999999997e-002,
            -2.4745400000000001e-001, -2.3696299999999999e+000),
            cv::Vec6d(3.9984300000000000e-001, -2.8360300000000001e-002,
            1.3857299999999999e-002, -2.3525799999999999e-003,
            -2.3726400000000000e-001, -2.4043199999999998e+000),
            cv::Vec6d(3.8515500000000003e-001, 7.5046299999999996e-002,
            5.9098299999999996e-003, 1.7270500000000000e-001,
            4.0474400000000001e-002, -2.2069200000000002e+000),
            cv::Vec6d(3.4627900000000000e-001, 1.7471500000000001e-001,
            -1.0372100000000000e-002, 9.3752400000000000e-002,
            1.8891700000000000e-001, -2.3043999999999998e+000),
            cv::Vec6d(2.7463900000000002e-001, 1.4251000000000000e-001,
            8.4143899999999994e-002, 1.2452100000000001e-001,
            1.9119800000000001e-001, -2.2954400000000001e+000),
            cv::Vec6d(2.7806700000000001e-001, 5.6496900000000003e-002,
            1.4376800000000001e-001, 4.6730500000000001e-002,
            6.5261899999999998e-002, -2.4095499999999999e+000),
            cv::Vec6d(4.1388399999999997e-001, 2.6377100000000001e-002,
            7.8375799999999995e-002, 1.0082500000000000e-001,
            -4.4208999999999998e-002, -2.3932799999999999e+000)
 #endif


//            cv::Vec6d(-0.517436,0.0210304,0.167021,-0.0346727,0.1937,-0.826195),
//            cv::Vec6d(-0.505066,0.111891,0.170868,0.204696,0.232034,-0.801171),
//            cv::Vec6d(-0.507533,-0.108507,0.155178,-0.279012,0.0248224,-0.731716),



        };

        cv::namedWindow(winname,cv::WINDOW_NORMAL);

        for(int i=0;i<10;i++)
        {

            cv::Vec6d urp=urps[i];
            URControl::SetTCPPose(ipUR,ipPC,urp[0],urp[1],urp[2],urp[3],urp[4],urp[5]);

            cv::waitKey(1000);
            cv::Mat m;

            for(int j=0;j<10;j++)
                c.read(m);

            cv::imshow(winname,m);
            cv::waitKey(1);

            cv::Vec6d urp1;
            URControl::GetTCPPose(ipUR,ipPC,urp1[0],urp1[1],urp1[2],urp1[3],urp1[4],urp1[5]);

            std::string name=TimeString();
            cv::imwrite(folder+name+ext,m);
            Save(cv::Mat(urp1),folder+name+".yml");

        }

        c.release();

        cv::destroyWindow(winname);

    }

    void Calibrate(std::string folder=TimeString(2)+"/")
    {
        std::cout<<"first calibrate camera position\n"
                <<"capture images save in "<<folder<<"\n"<<std::flush;
        //        SampleOne(folder, "press c to capture");

        Sample(folder);
        LoadCorners(folder);

        std::cout<<"now calibrate desk plane\n"
                <<"put ar code paper on the desk then press ESC\n"
               <<"press any key to start\n"<<std::flush;

        cv::waitKey();

        CalibratePlane();

    }




};

#endif // URCONTROLCAMERA_HPP
